#include "ClusterAlgorithm.hpp"

ClusterAlgorithm::ClusterAlgorithm() : SwarmAlgorithm() {
    id = "CLUSTER";
}

unsigned char ClusterAlgorithm::clusterAction(vector<Robot> &robots, int i, double offset) {
    if(i > -1)
    {
        double angle = robots[i].position.angle_rad;
        if(angle > offset)
        {
            return ACTION_TURN_LT;
        }
        if(angle < -offset)
        {
            return ACTION_TURN_RT;
        }
        if(angle > -offset && angle < offset)
        {
            return ACTION_FORWARD;
        }
    }
    else {
        return ACTION_STOP;
    }
}

unsigned char ClusterAlgorithm::clusterClosestAction(int color, double offset)
{
    vector<Robot> robots = robotFinder->getRobotPositions();
    int i = getClosestRobotIndex(robots, color);
    return clusterAction(robots, i, offset);
}
unsigned char ClusterAlgorithm::clusterFurthestAction(int color, double offset)
{
    vector<Robot> robots = robotFinder->getRobotPositions();
    int i = getFurthestRobotIndex(robots, color);
    return clusterAction(robots, i, offset);
}
unsigned char ClusterAlgorithm::clusterDirectionAction(double angle, int color)
{
	return ACTION_STOP;
}
unsigned char ClusterAlgorithm::clusterTrackAction(double rad, double angle, int color, double offset)
{
	vector<Robot> robots = robotFinder->getRobotPositions();

	double cpd, tcpd;//Temporary Combined Percent Difference
	double trad, tangle;//Temporary radius and angle 

	cpd = -1;
	bool foundMyColor = false;
    string colorStr = LED::intToColorString(color);
	for(int i = 0; i < robots.size(); i++)
	{
		if(color == LED::ANY || colorStr == robots[i].label)
		{
			foundMyColor = true;
			if(fabs(angle - robots[i].position.angle_rad) < 3.14)
			{
				tcpd = fabs(rad - robots[i].position.radius_cm)/rad +
				fabs(angle - robots[i].position.angle_rad)/(2*M_PI);//angle ;
			}
			else
			{
				tcpd = fabs(rad - robots[i].position.radius_cm)/rad +
				(2*M_PI - fabs(angle - robots[i].position.angle_rad))/(2*M_PI);//angle ;
			}
			if(cpd < 0 || tcpd < cpd)
			{
				trad   = robots[i].position.radius_cm;
				tangle = robots[i].position.angle_rad;
				cpd = tcpd;
			}
		}
	}
	if(foundMyColor)
	{
		rad = trad;
		angle = tangle;
		if(angle > offset)
		{
            return ACTION_TURN_LT;
		}
		if(angle < -offset)
		{
            return ACTION_TURN_RT;
		}
		if(angle > -offset && angle < offset)
		{
            return ACTION_FORWARD;
		}
	}
	else 
	{
        return ACTION_STOP;
	}
}


    
bool ClusterAlgorithm::setup() {
    return true;
}

void ClusterAlgorithm::loopBody() {
    if(bumperControl->isFrontTouching() || bumperControl->isBackTouching())
    {
        motorControl->stop();
        return;
    }

    unsigned char action = clusterClosestAction();
    if(action == ACTION_STOP)
    {
        action = getRandomMotorAction();
    }

    performAction(action, 5, 5);
}
